var Mt = math.matrix([
	[1, 0, 0],
	[0, 1, 0],
	[0, 0, 1]
]);
var xt;

var suanXY = function(coordinateX, coordinateY, currentStepLength, cDirection, jizhanX, jizhanY, qValue) {
	cDirection += 180;
	console.log('cDirection', cDirection);
	var cResultX, cResultY;
	cResultX = parseFloat(coordinateX) + parseFloat(currentStepLength * Math.sin(Math.PI / 180 * cDirection));
	cResultY = parseFloat(coordinateY) + parseFloat(currentStepLength * Math.cos(Math.PI / 180 * cDirection));

	var finalXY = JZ(cDirection, currentStepLength, cResultX, cResultY, jizhanX, jizhanY, qValue);
	return finalXY;
}
//矩阵滤波R1, R2, R3, R4 RSSI距离 r1, r2, r3, r4 PDR计算出的x,y到设备的距离 , distance：pdr一步移动距离
var JZ = function(direction, distance, dResultX, dResultY, jizhanX, jizhanY, qValue) {
	var SIN = Math.sin(Math.PI / 180 * direction);
	var COS = Math.cos(Math.PI / 180 * direction);
	var Z = math.matrix([
		[jizhanX],
		[jizhanY]
	]);
	console.log('Z', Z)

	var F = math.matrix([
		[1, 0, COS * distance],
		[0, 1, -SIN * distance],
		[0, 0, 1]
	]);

	var Ft = math.transpose(F);
	var R = null;
	//Q值来越大相信蓝牙，越小越相信pdr 1的时候完全相信蓝牙
	//R值来越大相信pdr，越小越相信蓝牙 0的时候完全相信蓝牙
	// var Q = math.matrix([[0.0001, 0, 0], [0, 0.0001, 0], [0, 0, 0.0001]]);
	//运动和静止两种状态两种不同Q
	var Q = null;

	Q = math.matrix([
		[0, 0, 0],
		[0, 0, 0],
		[0, 0, 0]
	]);
	R = math.matrix([
		[0, 0],
		[0, 0]
	]);
	//	if(sportState == 1) {
	//		Q = math.matrix([
	//			[0, 0, 0],
	//			[0, 0, 0],
	//			[0, 0, 0]
	////			[0.007, 0, 0],
	////			[0, 0.007, 0],
	////			[0, 0, 0.007]
	//		]);
	//		R = math.matrix([
	////			[0.001, 0],
	////			[0, 0.001]
	//			[0, 0],
	//			[0, 0]
	//		])
	//	} else {
	//		Q = math.matrix([
	//			[0, 0, 0],
	//			[0, 0, 0],
	//			[0, 0, 0]
	////			[0.007, 0, 0],
	////			[0, 0.007, 0],
	////			[0, 0, 0.007]
	//		]);
	//		R = math.matrix([
	//			[0, 0],
	//			[0, 0]
	////			[0.001, 0],
	////			[0, 0.001]
	//		])
	//	}
	var H = math.matrix([
		[1, 0, 0],
		[0, 1, 0]
	]);
	var ht = math.transpose(H);
	var I = math.matrix([
		[1, 0, 0],
		[0, 1, 0],
		[0, 0, 1]
	]);

	//第一个公式
	xt = math.matrix([
		[dResultX],
		[dResultY],
		[direction]
	]);
	//第二个公式
	var temp1 = math.multiply(F, Mt);
	var temp2 = math.multiply(temp1, Ft);
	// console.log('temp2', temp2)
	Mt = math.add(temp2, Q);
	// Mt = temp2;
	console.log('第二个公式Mt', Mt)
	//第三个公式
	var temp3 = math.multiply(Mt, ht); //第一个乘法
	var temp4 = math.multiply(H, Mt); //括号里的第一个乘法
	var temp5 = math.multiply(temp4, ht);
	var temp6 = math.add(temp5, R);
	var temp7 = math.inv(temp6);
	var Kt = math.multiply(temp3, temp7);
	console.log('第三个公式的Kt', Kt)
	//第四个公式
	var temp8 = math.multiply(H, xt); //括号里乘法
	var temp9 = math.subtract(Z, temp8);
	var temp10 = math.multiply(Kt, temp9);
	xt = math.add(xt, temp10);

	//第五个公式
	var temp11 = math.multiply(Kt, H); //括号里乘法
	var temp12 = math.subtract(I, temp11);
	Mt = math.multiply(temp12, Mt);
	console.log('第四个公式Mt', Mt)
	return xt;
}